/*! \file	calibrator.hpp
 *  \brief	Declarations for the CALIBRATOR node.
*/

#ifndef _THERMALVIS_CALIBRATOR_H_
#define _THERMALVIS_CALIBRATOR_H_
	
#include "ros_resources.hpp"
#include "general_resources.hpp"
#include "opencv_resources.hpp"

#include <signal.h>

#include "improc.hpp"
#include "video.hpp"
#include "calibration.hpp"
#include "intrinsics.hpp"
#include "extrinsics.hpp"
#include "features.hpp"



#include <ros/ros.h>
#include <boost/thread/thread.hpp>
#include <image_transport/image_transport.h>
#include <opencv/cv.h>
#include <opencv/highgui.h>


#include <dynamic_reconfigure/server.h>
#include "calibratorConfig.h"

#define MAX_ALLOWABLE_CAMS		2
#define DEFAULT_SET_SIZE		10
#define DEFAULT_GRID_SIZE		10 // (mm)
#define DEFAULT_X_COUNT			12
#define DEFAULT_Y_COUNT			8
#define DEFAULT_TIMER_PERIOD	0.05

#define DEFAULT_MAX_TIME_DIFF	0.001

//HGH
#define PATTERN_CODE_INVALID -1
#define PATTERN_CODE_FIND_TRACK 0
#define PATTERN_CODE_TRACK_FIND 1
#define PATTERN_CODE_FIND 2

const char __PROGRAM__[] = "thermalvis-calibrator";

bool wantsToShutdown = false;
void mySigintHandler(int sig);

/// \brief		Stores configuration information for the calibration routines
struct calibratorData {
	
	// Main inputs
	string read_addr;
	string video_stream, video_stream_secondary;
	string intrinsicsFiles[MAX_ALLOWABLE_CAMS];
		
	// Critical settings
	bool invert[MAX_ALLOWABLE_CAMS];
	string patternType, optMethod;
	double autoTimeout;
	bool trackingMode;
	string calibType;
	bool wantsIntrinsics;
	int pattType;
	int optimizationMethod;
	bool stopCapturing;
	string patternDetectionMode;
	bool useMutex;
	unsigned int numCams;
	
	bool fixPrincipalPoint;
	
	bool fixIntrinsics;
	
	bool noConstraints;
	
	bool ignoreDistortion, useRationalModel;
	
	double maxInterpolationTimegap, maxInterpolationMotion;
	
	// Course settings
	bool useUndistortedLocations;
	double alpha;
	bool autoAlpha;
	bool removeSpatialBias;
	
	// Fine settings
	double maxTimeDiff;
	int maxPatterns, maxFrames, setSize, maxCandidates, maxTests;
	double maxTime;
	double flowThreshold, errorThreshold, maxFrac;
	
	// Pattern properties
	double gridSize;
	int xCount, yCount;
	
	// Debug
	bool drawGrids;
	bool generateFigures;
	string outputFolder;
	bool debugMode;
	bool verboseMode;
	bool undistortImages;
	bool rectifyImages;
	
	// Camera models
	Mat K[MAX_ALLOWABLE_CAMS];
	Mat distCoeffs[MAX_ALLOWABLE_CAMS];
	
	// MSER settings
	bool adjustMSER[MAX_ALLOWABLE_CAMS];
	int delta[MAX_ALLOWABLE_CAMS];
	double maxVar[MAX_ALLOWABLE_CAMS], minDiv[MAX_ALLOWABLE_CAMS];
	double areaThreshold[MAX_ALLOWABLE_CAMS];
	
	// Functions
	bool obtainStartingData(ros::NodeHandle& nh);
	
};

/// \brief		Critical frame data regarding whether it contains a pattern valid for use in calibration
struct validPattern {
    int  validFrameID;
    int  patternID;
    bool patternFound;
    bool isValidFrame;
};


// example: http://ua-ros-pkg.googlecode.com/svn/trunk/arrg/ua_vision/object_tracking/src/object_tracker_node.cpp
/// \brief		Manages the calibration procedures
class calibratorNode {
private:

	ros::NodeHandle *ref;
	
	Mat prevMat[MAX_ALLOWABLE_CAMS], grayMat[MAX_ALLOWABLE_CAMS];

	struct timeval cycle_timer;
	double elapsedTime;
	double avgTime;
	
	struct timeval tracking_timer;
	double elapsedTrackingTime;
	double frameTrackingTime;
	
	unsigned int validSets;
	
	struct timeval vacant_timer, elapsed_timer;
	double vacantInputTime, elapsedInputTime;
	
	Mat map1[MAX_ALLOWABLE_CAMS], map2[MAX_ALLOWABLE_CAMS];
	int topValidHeight, botValidHeight, leftValid[MAX_ALLOWABLE_CAMS], rightValid[MAX_ALLOWABLE_CAMS];

	ros::Timer timer;
	
	vector<unsigned int> patternIndices[MAX_ALLOWABLE_CAMS];
	vector<Mat> displayImages[MAX_ALLOWABLE_CAMS];
	
	vector<ros::Time> times[MAX_ALLOWABLE_CAMS];
	
	vector<unsigned int> validPairs[MAX_ALLOWABLE_CAMS];
	
	
	
	bool infoProcessed[MAX_ALLOWABLE_CAMS];
	bool stillCollecting;

	double alpha;
	
	Mat default_R;
	
	Mat cameraMatrices[MAX_ALLOWABLE_CAMS];
	Mat distCoeffVecs[MAX_ALLOWABLE_CAMS];
	
	Rect roi[MAX_ALLOWABLE_CAMS];
	vector<Point2f> rectangleBounds, newRecBounds;
	Mat rectCamMat[MAX_ALLOWABLE_CAMS];
	vector<Point2f> leftLinePoints, rightLinePoints;
	
	Size imSize[MAX_ALLOWABLE_CAMS];
	Mat newCamMat[MAX_ALLOWABLE_CAMS];
	
	double reprojectionError_intrinsics[MAX_ALLOWABLE_CAMS];
	double extendedReprojectionError_intrinsics[MAX_ALLOWABLE_CAMS];
	
	vector<unsigned char> duplicateFlags[MAX_ALLOWABLE_CAMS];
	
	vector<ros::Time> patternTimestamps[MAX_ALLOWABLE_CAMS];
	
	vector<int> subselectedTags_intrinsics[MAX_ALLOWABLE_CAMS];
	
	string topic[MAX_ALLOWABLE_CAMS];
	
	unsigned int publishCount;
	
	// Extrinsics:
	Mat R_[MAX_ALLOWABLE_CAMS], P_[MAX_ALLOWABLE_CAMS];
	Mat E[MAX_ALLOWABLE_CAMS], F[MAX_ALLOWABLE_CAMS], Q[MAX_ALLOWABLE_CAMS];      // Between first camera and all other cameras
	Mat R[MAX_ALLOWABLE_CAMS], Rv[MAX_ALLOWABLE_CAMS], T[MAX_ALLOWABLE_CAMS];  // Rotations/translations between first camera and all other cameras
	Mat R2[MAX_ALLOWABLE_CAMS], T2[MAX_ALLOWABLE_CAMS];       // Rotations/translations between all other cameras
	
	double stereoError;
	double extendedExtrinsicReprojectionError;
	
	bool alphaChanged;

	calibratorData configData;
	
	sensor_msgs::CameraInfo debug_camera_info[MAX_ALLOWABLE_CAMS];

	image_transport::CameraPublisher debug_pub[MAX_ALLOWABLE_CAMS];
	
	image_transport::CameraSubscriber camera_sub[MAX_ALLOWABLE_CAMS];
	
	//ros::Subscriber info_sub, info_sub_1, info_sub_2;
	//image_transport::Subscriber image_sub, image_sub_1, image_sub_2;
	
	cv_bridge::CvImagePtr cv_ptr[MAX_ALLOWABLE_CAMS];
	
	char debug_pub_name[MAX_ALLOWABLE_CAMS][256];
	
	sensor_msgs::Image msg_debug[MAX_ALLOWABLE_CAMS];
	
	ros::NodeHandle private_node_handle;
	
	char nodeName[256];
	
	Mat newImage, lastImage;
	
	unsigned int checkIndex[MAX_ALLOWABLE_CAMS];
	unsigned int frameCount[MAX_ALLOWABLE_CAMS], undistortionCount, rectificationCount, totalFrameCount[2];
	
	cv::vector<Point3f> row;
    
	vector<vector<Point2f> > pointSets[MAX_ALLOWABLE_CAMS], candidateSets[MAX_ALLOWABLE_CAMS], testingSets[MAX_ALLOWABLE_CAMS];
	vector<vector<Point2f> > extrinsicsPointSets[MAX_ALLOWABLE_CAMS], extrinsicCandidateSets[MAX_ALLOWABLE_CAMS], extrinsicTestingSets[MAX_ALLOWABLE_CAMS];

        //HGH
        Mat lastMat[MAX_ALLOWABLE_CAMS];
        vector<Point2f> cornerSets[MAX_ALLOWABLE_CAMS];
        vector<unsigned int> frameCounts[MAX_ALLOWABLE_CAMS];
        bool  doVerify;

		
	boost::shared_mutex _access;
	
	bool readyForOutput;
	
	dynamic_reconfigure::Server<thermalvis::calibratorConfig> server;
	dynamic_reconfigure::Server<thermalvis::calibratorConfig>::CallbackType f;
	
	vector<Point2f> cornerSet[MAX_ALLOWABLE_CAMS];

        //HGH
        void preprocessImage(Mat src, Mat &dst, double a, double b, bool normaliz, bool negative );



public:
	calibratorNode(ros::NodeHandle& nh, calibratorData startupData);
	
	void performIntrinsicCalibration();
	void performExtrinsicCalibration();
	
	void handle_image(const sensor_msgs::ImageConstPtr& msg_ptr, const sensor_msgs::CameraInfoConstPtr& info_msg, const unsigned int camera_index = 0);
	
	
	void create_virtual_pairs();
	
	void assignDebugCameraInfo();
	
	void imageCallback(const sensor_msgs::ImageConstPtr& msg);
	
	void publishUndistorted(const ros::TimerEvent& event);
	void startUndistortionPublishing();
	
	void publishRectified(const ros::TimerEvent& event);
	void startRectificationPublishing();
	
	bool isStillCollecting();
	bool wantsToUndistort();
	bool wantsToRectify();
	void writeResults();
	bool wantsIntrinsics();
	bool wantsExtrinsics();
        void updatePairs();

        //HGH
        void determineValidPairs();
        void evaluateFrames();

	void getAverageTime();
	void getFrameTrackingTime();
	void assignIntrinsics();
	
	void preparePatternSubsets();
	void prepareExtrinsicPatternSubsets();
	
	void serverCallback(thermalvis::calibratorConfig &config, uint32_t level);
	void updateMap();
	
	bool findPattern(const Mat& im, vector<Point2f>& dst, Mat& prev, const int cameraNumber);
	
	void updateIntrinsicMap(unsigned int idx); 
	void set_ready_for_output();
	
	void prepareForTermination();

        //HGH
        bool isVerifying();

	void timerCallback(const ros::TimerEvent& e);

};

boost::shared_ptr < calibratorNode > *globalNodePtr;

#endif
